#include "clock.h"

static void CLOCK_pll_

void CLOCK_init() {
    // Set main oscillator range
    SCS_bit.OSCRANGE = osc_range;

    // Enable main oscillator
    SCS_bit.OSCEN = 1;

    // Wait until main oscillator settles
    while(!SCS_bit.OSCSTAT) {
        ; // do nothing
    }

    // Disconnect PLL
    PLLCON_bit.PLLC = 0;

    // Write PLL magic values (required for changes to take effect)
    PLLFEED = 0xAA;
    PLLFEED = 0x55;

    //  Disable PLL
    PLLCON_bit.PLLE = 0;
    
    // Write PLL magic values (required for changes to take effect)
    PLLFEED = 0xAA;
    PLLFEED = 0x55;
    
    // Select the main oscillator as PLL clock source
    CLKSRCSEL_bit.CLKSRC = 1;

    // Set PLL multiplier
    PLLCFG_bit.MSEL = pll_multiplier - 1;

    // Set PLL pre-divier
    PLLCFG_bit.NSEL = pll_pre_divider - 1;

    PLLFEED = 0xAA;
    PLLFEED = 0x55;
    // 7. Enable PLL
    PLLCON_bit.PLLE = 1;
    PLLFEED = 0xAA;
    PLLFEED = 0x55;
    // 8. Wait for the PLL to achieve lock
    while(!PLLSTAT_bit.PLOCK);
    // 9. Set clk divider settings
    CCLKCFG   = 5-1;            // 1/5 Fpll - 57.6 MHz
    USBCLKCFG = 6-1;            // 1/6 Fpll - 48 MHz
    PCLKSEL0 = PCLKSEL1 = 0;    // other peripherals - 18MHz
    // 10. Connect the PLL
    PLLCON_bit.PLLC = 1;
    PLLFEED = 0xAA;
    PLLFEED = 0x55;
}
